"""
机械手标定帮助
"""
import math

from params.robot_params import RobotParams
from vision_process.vision_helper import get_angle_by_k, get_two_points_middle


def get_rotate_angle01(xOffset, yOffset):
    """
    获取图像需要旋转的角度.机械手2的旋转角度,正值是顺时针旋转和图像的旋转角度是反的,所以要加上负号
    """
    if yOffset == 0:
        return 0
    angle = get_angle_by_k(yOffset / xOffset)
    return round(-angle, 3)


def get_rotate_angle02(xOffset, yOffset):
    """
    获取图像需要旋转的角度.机械手2的旋转角度,正值是顺时针旋转和图像的旋转角度是反的,所以要加上负号
    """
    if yOffset == 0:
        return 0
    angle = get_angle_by_k(yOffset / xOffset)
    return round(-angle, 3)


def calculate_rotated_offset(p1, p2, angle_deg, center=(0, 0)):
    """
    计算旋转后的坐标偏差变化
    :param p1: 点1坐标 (x1, y1)
    :param p2: 点2坐标 (x2, y2)
    :param angle_deg: 旋转角度（逆时针为正）
    :param center: 旋转中心，默认为原点
    :return: (新x偏差, 新y偏差)
    """
    # 原始偏差计算
    dx = p2[0] - p1[0]
    dy = p2[1] - p1[1]

    # 角度转弧度
    theta = math.radians(angle_deg)
    cos_theta = math.cos(theta)
    sin_theta = math.sin(theta)

    # 应用旋转矩阵[[cosθ, -sinθ], [sinθ, cosθ]]（网页1、71）
    new_dx = dx * cos_theta - dy * sin_theta
    new_dy = dx * sin_theta + dy * cos_theta

    return new_dx, new_dy


if __name__ == '__main__':
    # 机械手2的x偏移和y偏移
    xOffset02 = 235
    yOffset02 = -1.98
    angle02 = get_rotate_angle02(xOffset02, yOffset02)
    print("angle02: {:.3f}".format(angle02))

    xOffset01 = 235.27
    yOffset01 = 1.43
    angle01 = get_rotate_angle01(xOffset01, yOffset01)
    print("angle01: {:.3f}".format(angle01))

    rp = RobotParams()
    p1 = get_two_points_middle(rp.imageP1, rp.imageP2)
    print(p1)
    # 示例测试
    p2 = (1000, 1900)
    angle = rp.rotateAngle01  # 度

    original_dx = p2[0] - p1[0]  # 50
    original_dy = p2[1] - p1[1]  # 50

    new_dx, new_dy = calculate_rotated_offset(p1, p2, angle)
    print(f"原始偏差：dx={original_dx}, dy={original_dy}")
    print(f"旋转后偏差：dx≈{new_dx:.2f}, dy≈{new_dy:.2f}")
